#ifndef __CONTROLLER_P2P_H_
#define __CONTROLLER_P2P_H_

#include <mc_ros/visualization/controller_visualization.h>
#include <mc_ros/processor/state_machine.h>
#include "path_generate/path_base.h"
#include "controller/controller_base.h"


class ProcessorP2P : public StateMachineBase{
    public:
        ProcessorP2P(boost::shared_ptr<PlannerBase> path_planner_ptr,
            boost::shared_ptr<ControllerBase> localControllerPtr,
            boost::shared_ptr<MCVisualization> mcVisualizationPtr);
        ~ProcessorP2P();

        /********************
         *  虚函数实现
         ********************/
        // 设置目标点数据
        void setGoal(geometry_msgs::PoseStamped pose_stamp);
        // IDLE数据重置
        void smResetIdle();
        // 执行中预处理函数
        void smExecutingEntry();
        // 执行中处理函数
        void smExecutingRunning();
        // 执行中后处理函数
        void smExecutingExit();
        // 暂停中处理函数
        void smPauseRunning();
        // 取消中处理函数
        void smCancelRunning();
        // 结束中处理函数
        void smFinishedRunning();

    private:
        boost::shared_ptr<PlannerBase> globalPathPlannerPtr_;
        boost::shared_ptr<ControllerBase> localControllerPtr_;
        boost::shared_ptr<MCVisualization> mcVisualizationPtr_;

        geometry_msgs::PoseStamped goalPoseStamp_;
        bool __rotate_start_direction;
        bool setGoalPose_;
};

#endif